/*******************************************************************************
* This file implements the navigation algorithm for the UAV
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "navigation.h"



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static float prv_fGetDistanceFromTo(float fFromLat, float fFromLong, float fToLat, float fToLong);
static float prv_fGetBearingFromTo(float fFromLat, float fFromLong, float fToLat, float fToLong);

static float prv_fGetLoiterOffset(float fActualDistance, float fLoiterRadius, float fLoiterDir);
static float prv_fGetCrossTrackOffset(float fActualDistance, float fActualBearing, float fCrosstrackBearing);



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Position for home.
static float fHomeLat = 0.0f;
static float fHomeLong = 0.0f;
static float fHomeAltitude = 0.0f;

// Navigation data.
static float fHeadingSetpoint = 0.0f;		// The heading setpoint.
static float fBearingToTarget = 0.0f;		// Bearing from the current position to the target.
static float fDistanceToTarget = 0.0f;		// Distance from the current position to the target.

// Waypoints.
static struct NAV_COMMAND axCommandList[4];



/*******************************************************************************
* PUBLIC FUNCTION: fGetHeadingSetpoint
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The heading setpoint (0 - 2PI rad).
*
* DESCRIPTION:
* Get the heading setpoint of the UAV.
*
*******************************************************************************/
float fGetHeadingSetpoint(void)
{
	float fBuffer;
	
	INTERRUPT_PROTECT(fBuffer = fHeadingSetpoint);
	return fBuffer;
}	



/*******************************************************************************
* PUBLIC FUNCTION: fGetBearingToTarget
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The bearing to target (0 - 2PI rad).
*
* DESCRIPTION:
* Get the bearing from the current position to the target.
*
*******************************************************************************/
float fGetBearingToTarget(void)
{
	float fBuffer;
	
	INTERRUPT_PROTECT(fBuffer = fBearingToTarget);
	return fBuffer;
}	



/*******************************************************************************
* PUBLIC FUNCTION: fGetDistanceToTarget
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The distance to target (m).
*
* DESCRIPTION:
* Get the distance from the current position to the target.
*
*******************************************************************************/
float fGetDistanceToTarget(void)
{
	float fBuffer;
	
	INTERRUPT_PROTECT(fBuffer = fDistanceToTarget);
	return fBuffer;
}	



/*******************************************************************************
* TASK: taskNavigationLoop
*
* DESCRIPTIONS:
* This task is the main loop for the navigation calculation.
*
*******************************************************************************/
portTASK_FUNCTION(taskNavigationLoop, pvParameters)
{
	unsigned int uiGetNextCommand = 0;
	unsigned int uiCommandCount = 0;
	struct GPS_DATA xGpsData;
	struct NAV_COMMAND xCurrentNavCommand;
	float fDistanceToTargetTemp = 0.0f;
	float fBearingToTargetTemp = 0.0f;
	float fHeadingSetpointTemp = 0.0f;
	float fCrossTrackBearing = 0.0f;
	
	
	
	// Initialize the waypoints.
	axCommandList[0].eCommand = GOTO_FROM;
	axCommandList[0].xGotoFromParam.fFromLatitude = 5.265931f;
	axCommandList[0].xGotoFromParam.fFromLongitude = 100.434799f;
	axCommandList[0].xGotoFromParam.fLatitude = 5.265056f;
	axCommandList[0].xGotoFromParam.fLongitude = 100.431610f;
	axCommandList[0].xGotoFromParam.uiAltitude = 100;
	
	axCommandList[1].eCommand = GOTO;
	axCommandList[1].xGotoParam.fLatitude = 5.263867f;
	axCommandList[1].xGotoParam.fLongitude = 100.431936f;
	axCommandList[1].xGotoParam.uiAltitude = 100;
	
	axCommandList[2].eCommand = GOTO_FROM;
	axCommandList[2].xGotoFromParam.fFromLatitude = 5.263867f;
	axCommandList[2].xGotoFromParam.fFromLongitude = 100.431936f;
	axCommandList[2].xGotoFromParam.fLatitude = 5.264705f;
	axCommandList[2].xGotoFromParam.fLongitude = 100.435170f;
	axCommandList[2].xGotoFromParam.uiAltitude = 100;
	
	axCommandList[3].eCommand = GOTO;
	axCommandList[3].xGotoParam.fLatitude = 5.265931f;
	axCommandList[3].xGotoParam.fLongitude = 100.434799f;
	axCommandList[3].xGotoParam.uiAltitude = 100;
	
	
	// Initialize the first command.
	xCurrentNavCommand = axCommandList[0];
	
	
	// Wait until the GPS is locked and the number of satelites is > 8.
	do {
		// Let other task to run while waiting.
		// We check every 200ms.
		vTaskDelay(configTICK_RATE_HZ / 5);
		
		// Read the GPS data to check the status.
		xGpsData = xGetGpsData();
	}	
	while ((xGpsData.ucFixStatus == 0) || (xGpsData.uiNumberOfSatelites <= 8));
	
	
	
	// Save the current position as home.
	fHomeLat = xGpsData.fLatitude;
	fHomeLong = xGpsData.fLongitude;
	fHomeAltitude = 100.0f;
	
	
	
	// Indicate the navigation loop is ready.
	xSystemState.bNavigationReady = 1;
	
	
	
	// We need to initialize xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	while (1) {
	
		// Read the GPS data.
		xGpsData = xGetGpsData();
		
		
		
		// The command is loiter at home for infinite time.
		xCurrentNavCommand.eCommand = LOITER_COUNT;
		xCurrentNavCommand.xLoiterCountParam.fLatitude = fHomeLat;
		xCurrentNavCommand.xLoiterCountParam.fLongitude = fHomeLong;
		xCurrentNavCommand.xLoiterCountParam.uiAltitude = fHomeAltitude;
		xCurrentNavCommand.xLoiterCountParam.uiRadius = 100;
		xCurrentNavCommand.xLoiterCountParam.siLoiterDir = 1;
		xCurrentNavCommand.xLoiterCountParam.uiCount = 0;
		
		
		
		// Get the target latitude and longitude base on command type.
		float fTargetLat = 0.0f;
		float fTargetLong = 0.0f;
		unsigned int uiCalDistBearing = 0;
		
		switch (xCurrentNavCommand.eCommand) {
			case GOTO:
				fTargetLat = xCurrentNavCommand.xGotoParam.fLatitude;
				fTargetLong = xCurrentNavCommand.xGotoParam.fLongitude;
				
				// Indicate this command need to calculate the distance and bearing to target.
				uiCalDistBearing = 1;
				break;
				
			case GOTO_FROM:
				fTargetLat = xCurrentNavCommand.xGotoFromParam.fLatitude;
				fTargetLong = xCurrentNavCommand.xGotoFromParam.fLongitude;
				
				// Indicate this command need to calculate the distance and bearing to target.
				uiCalDistBearing = 1;
				break;
				
			case LOITER_COUNT:
				fTargetLat = xCurrentNavCommand.xLoiterCountParam.fLatitude;
				fTargetLong = xCurrentNavCommand.xLoiterCountParam.fLongitude;
				
				// Indicate this command need to calculate the distance and bearing to target.
				uiCalDistBearing = 1;
				break;
				
			case LOITER_TIME:
				fTargetLat = xCurrentNavCommand.xLoiterTimeParam.fLatitude;
				fTargetLong = xCurrentNavCommand.xLoiterTimeParam.fLongitude;
				
				// Indicate this command need to calculate the distance and bearing to target.
				uiCalDistBearing = 1;
				break;
				
			default:
				break;
		}	
		
		// Calculate the distance and bearing to target if the command required those information.
		if (uiCalDistBearing == 1) {
			// Calculate the distance from the current position to the target.
			fDistanceToTargetTemp = prv_fGetDistanceFromTo(xGpsData.fLatitude, xGpsData.fLongitude,
														   fTargetLat, fTargetLong);
			
			// Calculate the bearing from the source to target.
			// Only calculate if the distance is > 1 meter.
			if (fDistanceToTargetTemp > 1.0f) {
				fBearingToTargetTemp = prv_fGetBearingFromTo(xGpsData.fLatitude, xGpsData.fLongitude,
															 fTargetLat, fTargetLong);
			}
		}	
		
		
		
		// Execute the command.
		switch (xCurrentNavCommand.eCommand) {
			
			case GOTO:
				// Calculate the heading setpoint.
				fHeadingSetpointTemp = fBearingToTargetTemp;
				
				// If we are getting near to the target, set the flag to get next command.
				if (fDistanceToTargetTemp < MIN_DISTANCE) {
					uiGetNextCommand = 1;
				}	
				
				break;
			
			
			
			case GOTO_FROM:
				// Calculate the cross track bearing.
				fCrossTrackBearing = prv_fGetBearingFromTo(xCurrentNavCommand.xGotoFromParam.fFromLatitude,
														   xCurrentNavCommand.xGotoFromParam.fFromLongitude,
														   xCurrentNavCommand.xGotoFromParam.fLatitude,
														   xCurrentNavCommand.xGotoFromParam.fLongitude);
				
				// Calculate the heading setpoint.
				fHeadingSetpointTemp = fBearingToTargetTemp +
									   prv_fGetCrossTrackOffset(fDistanceToTargetTemp,
									   							fBearingToTargetTemp,
									   							fCrossTrackBearing);
				
				// If we are getting near to the target, set the flag to get next command.
				if (fDistanceToTargetTemp < MIN_DISTANCE) {
					uiGetNextCommand = 1;
				}	
				
				break;
			
			
			
			case LOITER_COUNT:	
				// Calculate the heading setpoint.
				fHeadingSetpointTemp = fBearingToTargetTemp +
									   prv_fGetLoiterOffset(fDistanceToTargetTemp,
									   						(float)xCurrentNavCommand.xLoiterCountParam.uiRadius,
									   						(float)xCurrentNavCommand.xLoiterCountParam.siLoiterDir);
				
				break;
			
			
			
			case LOITER_TIME:	
				// Calculate the heading setpoint.
				fHeadingSetpointTemp = fBearingToTargetTemp +
									   prv_fGetLoiterOffset(fDistanceToTargetTemp,
									   						(float)xCurrentNavCommand.xLoiterCountParam.uiRadius,
									   						(float)xCurrentNavCommand.xLoiterCountParam.siLoiterDir);
				
				break;
		}	
		
		
		
		// Convert the angle to 0 - 2PI rad.
		CONVERT_0_2PI(fHeadingSetpointTemp);
		
		// Update the global variable.
		INTERRUPT_PROTECT(	fHeadingSetpoint = fHeadingSetpointTemp;	\
							fBearingToTarget = fBearingToTargetTemp;	\
							fDistanceToTarget = fDistanceToTargetTemp;
						 );
		
		
		
		// Change to next command if we finish executing the current command.
		if (uiGetNextCommand == 1) {
			uiGetNextCommand = 0;
			
			if (++uiCommandCount >= 4) {
				uiCommandCount = 0;
			}	
			
			xCurrentNavCommand = axCommandList[uiCommandCount];
		}	
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiNavigationLoop = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiNavigationLoop < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
		
		// Loop every 100ms (10 Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 10);
	}
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_fGetDistanceFromTo
*
* PARAMETERS:
* ~ fFromLat	- Source latitude.
* ~ fFromLong	- Source longitude.
* ~ fToLat		- Target latitude.
* ~ fToLong		- Target longitude.
*
* RETURN:
* ~ The distance from source to target in meters.
*
* DESCRIPTION:
* Calculate the distance from the source to target.
*
*******************************************************************************/
static float prv_fGetDistanceFromTo(float fFromLat, float fFromLong, float fToLat, float fToLong)
{
	// Calculate the scale for sinking longitude towards the pole.
	float fLonScale = cosf(fFromLat * DEG_TO_RAD);
	
	// Calculate the delta latitude and longitude between the current position
	// and the waypoint. The values need to be in radian.
	float fDeltaLatitude = fToLat - fFromLat;
	float fDeltaLongitude = (fToLong - fFromLong) * fLonScale;
		
	// Calculate the distance from the current position to the waypoint.
	return sqrtf((fDeltaLatitude * fDeltaLatitude) + (fDeltaLongitude * fDeltaLongitude)) * COORD_DEG_TO_DIST;
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_fGetBearingFromTo
*
* PARAMETERS:
* ~ fFromLat	- Source latitude.
* ~ fFromLong	- Source longitude.
* ~ fToLat		- Target latitude.
* ~ fToLong		- Target longitude.
*
* RETURN:
* ~ The bearing from source to target in radian (0 - 2PI rad).
*
* DESCRIPTION:
* Calculate the bearing to from source to target.
*
*******************************************************************************/
static float prv_fGetBearingFromTo(float fFromLat, float fFromLong, float fToLat, float fToLong)
{
	// Calculate the scale for sinking longitude towards the pole.
	float fLonScale = cosf(fFromLat * DEG_TO_RAD);
	
	// Calculate the delta latitude and longitude between the source
	// and target. The values need to be in radian.
	float fDeltaLatitude = fToLat - fFromLat;
	float fDeltaLongitude = (fToLong - fFromLong) * fLonScale;
	
	// Calculate the bearing from the source to target if not both the delta latitude and longitude are 0.
	float fBearing = 0.0f;
	if ((fDeltaLongitude != 0.0f) || (fDeltaLatitude != 0.0f)) {
		fBearing = atan2f(fDeltaLongitude, fDeltaLatitude);
	}	
	
	// Convert the angle to 0 - 360 degree.
	CONVERT_0_2PI(fBearing);
	
	return fBearing;
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_fGetLoiterOffset
*
* PARAMETERS:
* ~ fActualDistance		- Current distance to the target (m).
* ~ fLoiterRadius		- Loiter radius (m).
* ~ fLoiterDir			- Loiter direction (1 = CW, -1 = CCW).
*
* RETURN:
* ~ The heading offset (rad).
*
* DESCRIPTION:
* Calculate the heading offset for the loiter.
*
*******************************************************************************/
static float prv_fGetLoiterOffset(float fActualDistance, float fLoiterRadius, float fLoiterDir)
{
	float fHeadingOffset = 0.0f;
	float fOffsetRatio = 0.0f;
			
	// If we are inside the loiter circle...
	if (fActualDistance <= fLoiterRadius){
		fOffsetRatio = fActualDistance / fLoiterRadius;
		
		// Add to the heading setpoint.
		fHeadingOffset = (2.0f + fOffsetRatio) * PI / 2.0 * fLoiterDir;
	}
	
	// We are getting near to the loiter circle...
	else if (fActualDistance < (fLoiterRadius + LOITER_RANGE)) {
		fOffsetRatio = ((fLoiterRadius + LOITER_RANGE) - fActualDistance) / LOITER_RANGE;
		
		// Add to the heading setpoint.
		fHeadingOffset = -(fOffsetRatio * PI / 2.0f * fLoiterDir);
	}
	
	// We are still far from the target.
	else {
		fHeadingOffset = 0.0f;
	}	
	
	return fHeadingOffset;
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_fGetCrossTrackOffset
*
* PARAMETERS:
* ~ fActualDistance		- Current distance to the target (m).
* ~ fActualBearing		- Current bearing to the target (rad).
* ~ fCrosstrackBearing	- Bearing for the track (rad).
*
* RETURN:
* ~ The heading offset (rad).
*
* DESCRIPTION:
* Calculate the heading offset for the cross track.
*
*******************************************************************************/
static float prv_fGetCrossTrackOffset(float fActualDistance, float fActualBearing, float fCrosstrackBearing)
{
	float fHeadingOffset = 0.0f;
	
	// Calculate the crosstrack error and offset the error to within +-PI.
	float fCrosstrackError = fActualBearing - fCrosstrackBearing;
	CONVERT_NEGPI_PI(fCrosstrackError);
	
	// Calculate how far we are from the track.
	float fDist2Track = sinf(fCrosstrackError) * fActualDistance;
	
	// Calculate and limit the heading offset that we need to add to the heading setpoint.
	fHeadingOffset = LIMIT(xSystemConfig.fCrossTrackKp * fDist2Track, -CROSSTRACK_OFFSET_MAX, CROSSTRACK_OFFSET_MAX);
	
	return fHeadingOffset;
}	
